#ifndef ANDRES_VECTORIZATION_SIGN_VEC_H
#define ANDRES_VECTORIZATION_SIGN_VEC_H

#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Core>
#include <base/defines.h>
#include <base/camera.h>
#include <base/vec.h>
#include <vector>
#include <string>
#include <map>

namespace ville3d
{
    struct ImagePair
    {
        std::size_t left, right;
        double cosAngle;
    };

    class SignVec
    {
    public:
        SignVec(const std::string& img_path);
        virtual ~SignVec();
        std::vector<Eigen::Vector3d> DoWork1(SignBatch& signBatch, std::map<std::string, Pose>& poseMap, const Camera& camera);
        std::vector<Eigen::Vector3d> DoWork(SignBatch& signBatch, std::map<std::string, Pose>& poseMap, const Camera& camera);
    protected:
        void Dump(const SignBatch& signBatch, const Camera& camera);
        bool triangulateView(const SignVecMeta& to_vec1, const SignVecMeta& to_vec2, const Camera& camera, Eigen::Vector3d& vec3d);
        void refineVertexs(const std::vector<SignVecMeta>& to_vecs, const Camera& camera, std::vector<Eigen::Vector3d>& vec3ds);
        double calculateDepth(const Pose& pose, const Camera& camera, Eigen::Vector3d& vec3d) const;
        std::vector<Eigen::Vector3d> reprojectByDepth(const SignVecMeta& signVec, const Camera& camera, double depth) const;
        bool refineDepth(const std::vector<SignVecMeta>& to_vecs, const Camera& camera, double& depth) const;
        std::vector<ImagePair> sortViewsForBaseline(const std::vector<SignVecMeta>& to_vecs) const;
        void undistortbBox(std::vector<SignVecMeta>& signBatch, const Camera& camera);

    private:
        double rayLength_;
        std::string image_root_;
    };
} // namespace Ville3d

#endif